ROS2 node

We provide a unique ROS2 node to use WOLF in ROS2. This node is generic in the sense that it will serve many different robotics projects without modification. This means that all the user has to do is:

• Write a YAML file or a set of YAML files specifying all the problem parameters.

• Write a launch file for your application specifying the paths to your YAML files and installed WOLF plugins and subscribers.

• Launch and enjoy.

See also

See the YAML configuration section for more information on configuring a ROS2 application with WOLF.

This unique ROS2 node is implemented in the wolf_ros2_node package.

Description of the node

A quick overview of the ROS node follows:

namespace wolf
{
class Node : public rclcpp::Node
{
  public:
    // subscribers
    std::list<SubscriberBasePtr> subscribers_;
    // publishers
    std::list<PublisherBasePtr> publishers_;
    // sensor switcher
    SensorSwitcherPtr sensor_switcher_;

  protected:
    YAML::Node       params_;       ///< params
    ProblemPtr       problem_ptr_;  ///< wolf problem
    SolverManagerPtr solver_;       ///< solver

    // profiling
    bool                                                        profiling_;
    std::ofstream                                               profiling_file_;
    std::chrono::time_point<std::chrono::high_resolution_clock> start_experiment_;

    // print
    bool         print_problem_;
    double       print_period_;
    rclcpp::Time last_print_;
    int          print_depth_;
    bool         print_factored_by_, print_metric_, print_state_blocks_;

  public:
    double node_rate_;

    Node();

    virtual ~Node(){};

    void initialize();

    SolverManagerPtr getSolver();

    void print();
    void createProfilingFile();
};

}  // namespace wolf

where the principal bits are briefly described below:

The node constructor & initialize()

The node constructor uses WOLF autoconf to:

1. Read the path of the main YAML file provided through the ROS parameter yaml_file_path.

2. Load the YAML file and search the required plugins and packages.

3. Load all required WOLF plugins and ROS2 packages and store their schema folders for validation.

4. Validate the YAML configuration against the loaded schemas.

5. Configure the WOLF problem as specified (dimension, sensors, processors, tree manager, map…)

6. Configure a solver for the factor graph.

7. Configure other features such as node rate, profiling options and printing options.

Then, the initialize() method is called to:

1. Create a series of ROS2 subscribers with callbacks, one per sensor, as specified in the YAML config file.

2. Create a series of ROS2 publishers with topic and specified frequency in the YAML config file.

3. Create a sensor switcher to enable/disable sensors on the fly.

solve() & solveLoop()

The solve() method calls the solveLoop() periodically to optimize the factor graph and if specified, compute the covariance of parts of the state. It is executed in an dedicated thread with high priority to ensure that the solver is not delayed by other processes.

The main() routine

In ǹode.cpp, you can find the main() routine:

int main(int argc, char **argv)
{
    // Init ROS
    rclcpp::init(argc, argv);

    // Wolf node
    std::shared_ptr<wolf::Node> wolf_node = std::make_shared<wolf::Node>();

    // Initialize publishers and subscribers
    wolf_node->initialize();

    rclcpp::Rate loopRate(wolf_node->node_rate_);

    // periodic stuff
    rclcpp::Time last_check = wolf_node->get_clock()->now();

    // Init solver thread
    wolf_node->getSolver()->run();

    // Init publishers threads
    for (auto pub : wolf_node->publishers_)
    {
        WOLF_INFO("Running publisher ", pub->getType(), "(", pub->getTopic(), ")");
        pub->run();
    }

    while (rclcpp::ok())
    {
        // check that subscribers received data (every second)
        if ((wolf_node->get_clock()->now() - last_check).seconds() > 1)
        {
            for (auto sub : wolf_node->subscribers_)
                WOLF_WARN_COND(sub->secondsSinceLastCallback() > 5,
                               sub->getType(),
                               "(",
                               sub->getTopic(),
                               ")",
                               " has not received any callback for ",
                               sub->secondsSinceLastCallback(),
                               "s.");
            last_check = wolf_node->get_clock()->now();
        }

        // print periodically
        wolf_node->print();

        // execute pending callbacks
        rclcpp::spin_some(wolf_node);

        // relax to fit output rate
        loopRate.sleep();
    }

    // Stop solver thread
    WOLF_DEBUG("Node is shutting down outside loop... waiting for the solver thread to stop...");
    wolf_node->getSolver()->stop();
    WOLF_DEBUG("thread stopped.");

    // Stop publishers threads
    WOLF_DEBUG("waiting for the publishers threads to stop...");
    for (auto pub : wolf_node->publishers_) pub->stop();
    WOLF_DEBUG("threads stopped.");

    // PROFILING
    wolf_node->createProfilingFile();

    rclcpp::shutdown();

    return 0;
}

Some important remarks:

• The subscribers are called sequentially in the main thread. Note that inside the subscribers, a capture of the corresponding type will be created and processed.

• The publishers have their dedicated threads.